//
// Created by meng on 2021/2/19.
//
#include "eskf.h"
#include "../3rd/sophus/se3.hpp"

constexpr double kDegree2Radian = M_PI / 180.0;

Eigen::Matrix3d BuildSkewMatrix(const Eigen::Vector3d& vec){
    Eigen::Matrix3d matrix;
    matrix << 0.0,     -vec[2],   vec[1],
              vec[2],    0.0,     -vec[0],
              -vec[1],   vec[0],    0.0;

    return matrix;
}

ESKF::ESKF(const YAML::Node &node) {
    double gravity = node["earth"]["gravity"].as<double>();
    double earth_rotation_speed = node["earth"]["rotation_speed"].as<double>();
    double cov_prior_posi = node["covariance"]["prior"]["posi"].as<double>();
    double cov_prior_vel = node["covariance"]["prior"]["vel"].as<double>();
    double cov_prior_ori = node["covariance"]["prior"]["ori"].as<double>();
    double cov_prior_epsilon = node["covariance"]["prior"]["epsilon"].as<double>();
    double cov_prior_delta = node["covariance"]["prior"]["delta"].as<double>();
    double cov_measurement_posi = node["covariance"]["measurement"]["posi"].as<double>();
    double cov_process_gyro = node["covariance"]["process"]["gyro"].as<double>();
    double cov_process_accel = node["covariance"]["process"]["accel"].as<double>();
    L_ = node["earth"]["latitude"].as<double>();
    g_ = Eigen::Vector3d(0.0, 0.0, -gravity);
    w_ = Eigen::Vector3d(0.0, earth_rotation_speed * cos(L_ * kDegree2Radian),
                         earth_rotation_speed * sin(L_ * kDegree2Radian));

    SetCovarianceP(cov_prior_posi, cov_prior_vel, cov_prior_ori,
                   cov_prior_epsilon, cov_prior_delta);
    SetCovarianceR(cov_measurement_posi);
    SetCovarianceQ(cov_process_gyro, cov_process_accel);

    X_.setZero();
    F_.setZero();
    C_.setIdentity();
	//C^n是当前t时刻，IMU body frame到导航系n的变换
	/*
	 *		｜０3x3 I3x3 ０3x3 	   ０3x3 ０3x3｜
	 *		｜０3x3 ０3x3 F3x3(23) ０3x3 C^nb｜
	 * Ft = ｜０3x3 ０3x3 F3x3(33) -C^nb ０3x3｜
	 *		｜０3x3 ０3x3 ０3x3    ０3x3 ０3x3｜
	 *		｜０3x3 ０3x3 ０3x3    ０3x3 ０3x3｜
	 */
	//东北天加速度
	/*		  	 ｜0 	-fU fN ｜
	 * F3x3(23)=｜fU 	0  -fe ｜
	 *		  	 ｜-fN	0	0  ｜
	 */ 
	//自转角速度
	/*		  	 ｜0 		wsinL -wcosL ｜
	 * F3x3(33)= ｜-wSinL 	0  		   0 ｜
	 *		  	 ｜-fN	0	0 		   0 ｜ 
	 */ 
    G_.block<3,3>(INDEX_MEASUREMENT_POSI,INDEX_MEASUREMENT_POSI) = Eigen::Matrix3d::Identity();

    F_.block<3,3>(INDEX_STATE_POSI, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity();
    F_.block<3,3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = BuildSkewMatrix(-w_);
}

void ESKF::SetCovarianceQ(double gyro_noise, double accel_noise) {
    Q_.setZero();
    Q_.block<3,3>(0,0) = Eigen::Matrix3d::Identity() * gyro_noise * gyro_noise;
    Q_.block<3,3>(3,3) = Eigen::Matrix3d::Identity() * accel_noise * accel_noise;
}

void ESKF::SetCovarianceR(double posi_noise) {
    R_.setZero();
    R_ = Eigen::Matrix3d::Identity() * posi_noise * posi_noise;
}

void ESKF::SetCovarianceP(double posi_noise, double velo_noise, double ori_noise,
                          double gyro_noise, double accel_noise) {
    P_.setZero();
    P_.block<3,3>(INDEX_STATE_POSI, INDEX_STATE_POSI) = Eigen::Matrix3d::Identity() * posi_noise;
    P_.block<3,3>(INDEX_STATE_VEL, INDEX_STATE_VEL) = Eigen::Matrix3d::Identity() * velo_noise;
    P_.block<3,3>(INDEX_STATE_ORI, INDEX_STATE_ORI) = Eigen::Matrix3d::Identity() * ori_noise;
    P_.block<3,3>(INDEX_STATE_GYRO_BIAS, INDEX_STATE_GYRO_BIAS) = Eigen::Matrix3d::Identity() * gyro_noise;
    P_.block<3,3>(INDEX_STATE_ACC_BIAS, INDEX_STATE_ACC_BIAS) = Eigen::Matrix3d::Identity() * accel_noise;
}

bool ESKF::Init(const GPSData &curr_gps_data, const IMUData &curr_imu_data) {
    init_velocity_ = curr_gps_data.true_velocity;
    velocity_ = init_velocity_;
    //北东地->东北天
    Eigen::Quaterniond Q = Eigen::AngleAxisd(90 * kDegree2Radian, Eigen::Vector3d::UnitZ()) *
                           Eigen::AngleAxisd(0 * kDegree2Radian, Eigen::Vector3d::UnitY()) *
                           Eigen::AngleAxisd(180 * kDegree2Radian, Eigen::Vector3d::UnitX());
    init_pose_.block<3,3>(0,0) = Q.toRotationMatrix();
    pose_ = init_pose_;

    imu_data_buff_.clear();
    imu_data_buff_.push_back(curr_imu_data);

    curr_gps_data_ = curr_gps_data;

    return true;
}

void ESKF::GetFGY(TypeMatrixF &F, TypeMatrixG &G, TypeVectorY &Y) {
    F = Ft_;
    G = G_;
    Y = Y_;
}

//多来一帧的GPS信号就对位姿进行一下更新
bool ESKF::Correct(const GPSData &curr_gps_data) 
{
    //更新gps的参数
    curr_gps_data_ = curr_gps_data;
	//使用预积分得到的结果减去gps绝对的结果得到一个误差
    Y_ = pose_.block<3,1>(0,3) - curr_gps_data.position_ned;
	//进行卡尔曼滤波的经典三个步骤
	//1)计算卡尔曼增益 卡尔曼步骤(3)
	/**
	 * S=HP'H^T+R
	 * K=P'H^tS^-1
	 */
    K_ = P_ * G_.transpose() * (G_ * P_ * G_.transpose() + C_ * R_ * C_.transpose()).inverse();
	//观测更新协防差矩阵，卡尔曼更新步骤(4)
	/*
	 *	x=x'+Ky
	 *	P=(I-KH)P';
	 *	这两个公式，实际上完成了卡尔曼滤波的闭环，第一个公式是完成了当前状态向量x更新，不仅考虑了上一时刻的预测值，也考虑了测量值，和整个系统的噪声
	 *  第二个公式根据卡尔曼增益，更新了系统不确定程度P,用于下一个周期的运算．该公式中的I为与状态向量同维度的单位矩阵．
	 */
    P_ = (TypeMatrixP::Identity() - K_ * G_) * P_;
	//对状态进行更新，卡尔曼更新步骤(5)
    X_ = X_ + K_ * (Y_ - G_ * X_);
	//利用后验结果对已有状态进行误差消除
    EliminateError();
	//全部状态归零
    ResetState();

    return true;
}

bool ESKF::Predict(const IMUData &curr_imu_data) 
{
    imu_data_buff_.push_back(curr_imu_data);

    UpdateOdomEstimation();
	//计算该帧和最前面的帧之间的时间差
    double delta_t = curr_imu_data.time - imu_data_buff_.front().time;
	//将body系的坐标系转换到全局坐标系下
    Eigen::Vector3d curr_accel = pose_.block<3, 3>(0, 0)* curr_imu_data.linear_accel;
	//根据时间间隔t和全局下的加速度进行预测更新，对应的(1)(2)步骤
    UpdateErrorState(delta_t, curr_accel);

    imu_data_buff_.pop_front();
}

bool ESKF::UpdateErrorState(double t, const Eigen::Vector3d &accel) 
{
    Eigen::Matrix3d F_23 = BuildSkewMatrix(accel);
    F_.block<3,3>(INDEX_STATE_VEL, INDEX_STATE_ORI) = F_23;
    F_.block<3,3>(INDEX_STATE_VEL, INDEX_STATE_ACC_BIAS) = pose_.block<3,3>(0,0);
    F_.block<3,3>(INDEX_STATE_ORI, INDEX_STATE_GYRO_BIAS) = -pose_.block<3,3>(0,0);

	/*
	 *		｜０3x3 ０3x3｜
	 * Bt = ｜０3x3 Cnxb ｜
	 *		｜-Cnxb ０3x3｜ 
	 *		｜０3x3 ０3x3｜ 
	 */
    B_.block<3,3>(INDEX_STATE_VEL, 3) = pose_.block<3,3>(0,0);
    B_.block<3,3>(INDEX_STATE_ORI, 0) = -pose_.block<3,3>(0,0);
	//计算相对时间差内的变化DetF
    TypeMatrixF Fk = TypeMatrixF::Identity() + F_ * t;
	//计算相对时间差内的噪声变化角度
    TypeMatrixB Bk = B_ * t;
    Ft_ = F_ * t;

	//这里的x为状态向量，通过左乘一个矩阵F,再加上外部的影响u,得道预测的状态向量x'.这里的F成为状态转移矩阵．
    X_ = Fk * X_;//卡尔曼更新步骤(1),误差也没给只能当作准的．
	//该公式中的P表示系统的不确定程度，这个不确定程度，在卡尔曼滤波器初始化时会很大，随着越来越多的数据注入滤波器
	//中，不确定程度会变小，P的专业术语叫做状态协防差矩阵,这里的Q表示过程噪声，即无法用x'=Fx+u表示的噪声，比如车辆
	//运动时突然到了上坡，这个影响是无法用之前的状态转移估计的
    P_ = Fk * P_ * Fk.transpose() + Bk * Q_ * Bk.transpose();//卡尔曼更新步骤(2),方差项可以更新根据BK
    return true;
}

bool ESKF::UpdateOdomEstimation() {
    Eigen::Vector3d angular_delta;
	//计算相对转动了多少,有前后两帧的角速度，速度一半乘以时间就可以了
    ComputeAngularDelta(angular_delta);

	//计算地球转动带来的影响,计算两帧之间地球转动的角度
    Eigen::Matrix3d R_nm_nm_1;
    ComputeEarthTranform(R_nm_nm_1);
    
	//将之前的旋转矩阵和现在的旋转矩阵求出来，计算旋转的部分
	//更新pose中的旋转向量部分，一个是转动角度，一个是地球的角度
	//last_R,上帧的R,curr_R imu更新的R
    Eigen::Matrix3d curr_R, last_R;
    ComputeOrientation(angular_delta, R_nm_nm_1, curr_R, last_R);
	//将上一帧的速度和当前帧的速度计算出来
    Eigen::Vector3d curr_vel, last_vel;
    ComputeVelocity(curr_vel, last_vel, curr_R, last_R);
	//计算pose中位移的部分,计算当前位置
    ComputePosition(curr_vel, last_vel);

    return true;
}

//计算两帧IMU之间的转动角度，有前后两帧的角速度，速度均值乘以时间即可
bool ESKF::ComputeAngularDelta(Eigen::Vector3d &angular_delta) 
{
    IMUData curr_imu_data = imu_data_buff_.at(1);
    IMUData last_imu_data = imu_data_buff_.at(0);

    double delta_t = curr_imu_data.time - last_imu_data.time;

    if(delta_t <= 0)
	{
        return false;
    }

    Eigen::Vector3d curr_angular_vel = curr_imu_data.angle_velocity;
    Eigen::Vector3d last_angular_vel = last_imu_data.angle_velocity;

    Eigen::Vector3d curr_unbias_angular_vel = curr_angular_vel;
    Eigen::Vector3d last_unbias_angular_vel = last_angular_vel;

	//通过两帧的角速度计算出的偏角
    angular_delta = 0.5 * (curr_unbias_angular_vel + last_unbias_angular_vel) * delta_t;
    return true;
}
//w=[0.0,w*cos(L),w*sin(L)]
//计算地球转动带来的影响，计算两帧之间地球转动角度
bool ESKF::ComputeEarthTranform(Eigen::Matrix3d &R_nm_nm_1)
{
    IMUData curr_imu_data = imu_data_buff_.at(1);
    IMUData last_imu_data = imu_data_buff_.at(0);

    double delta_t = curr_imu_data.time - last_imu_data.time;

    constexpr double rm = 6353346.18315;//短半轴
    constexpr double rn = 6384140.52699;//长半轴
    Eigen::Vector3d w_en_n(-velocity_[1] / (rm + curr_gps_data_.position_lla[2]),
                           velocity_[0] / (rn + curr_gps_data_.position_lla[2]),
                           velocity_[0] / (rn + curr_gps_data_.position_lla[2])
                           * std::tan(curr_gps_data_.position_lla[0] * kDegree2Radian));

    Eigen::Vector3d w_in_n = w_en_n + w_;//当前位置的一个地球转动角速度

    auto angular = delta_t * w_in_n;//计算出时间delta_t内的转动角度

    Eigen::AngleAxisd angle_axisd(angular.norm(), angular.normalized());//角度模长以及归一化向量

    R_nm_nm_1 = angle_axisd.toRotationMatrix().transpose();
}
//更新pose中的旋转向量部分，一是转动的角度，一个是地球角度
bool ESKF::ComputeOrientation(const Eigen::Vector3d &angular_delta,
                              const Eigen::Matrix3d R_nm_nm_1,
                              Eigen::Matrix3d &curr_R,
                              Eigen::Matrix3d &last_R) {
    Eigen::AngleAxisd angle_axisd(angular_delta.norm(), angular_delta.normalized());
    last_R = pose_.block<3, 3>(0, 0);

    curr_R = R_nm_nm_1 * pose_.block<3, 3>(0, 0) * angle_axisd.toRotationMatrix();

    pose_.block<3, 3>(0, 0) = curr_R;

    return true;
}
//计算当前帧的速度和上一帧的速度
bool ESKF::ComputeVelocity(Eigen::Vector3d &curr_vel, Eigen::Vector3d& last_vel,
                                             const Eigen::Matrix3d &curr_R,
                                             const Eigen::Matrix3d last_R) {
    IMUData curr_imu_data = imu_data_buff_.at(1);
    IMUData last_imu_data = imu_data_buff_.at(0);
    double delta_t = curr_imu_data.time - last_imu_data.time;
    if (delta_t <=0 ){
        return false;
    }

    Eigen::Vector3d curr_accel = curr_imu_data.linear_accel;
    Eigen::Vector3d curr_unbias_accel = GetUnbiasAccel(curr_R * curr_accel);//就是把地球的重力加速度加上，转化到了导航坐标系

    Eigen::Vector3d last_accel = last_imu_data.linear_accel;
    Eigen::Vector3d last_unbias_accel = GetUnbiasAccel(last_R * last_accel);

    last_vel = velocity_;

    velocity_ += delta_t * 0.5 * (curr_unbias_accel + last_unbias_accel);
    curr_vel = velocity_;

    return true;
}

Eigen::Vector3d ESKF::GetUnbiasAccel(const Eigen::Vector3d &accel) {
//    return accel - accel_bias_ + g_;
    return accel + g_;
}

bool ESKF::ComputePosition(const Eigen::Vector3d& curr_vel, const Eigen::Vector3d& last_vel)
{
    double delta_t = imu_data_buff_.at(1).time - imu_data_buff_.at(0).time;
    pose_.block<3,1>(0,3) += 0.5 * delta_t * (curr_vel + last_vel);
    return true;
}

void ESKF::ResetState() {
    X_.setZero();
}

//利用后验结果对已有的状态进行误差消除
void ESKF::EliminateError() {
	//位姿［0 3］减去第一个
    pose_.block<3,1>(0,3) = pose_.block<3,1>(0,3) - X_.block<3,1>(INDEX_STATE_POSI, 0);
	//速度［3 6］减去第二个
    velocity_ = velocity_ - X_.block<3,1>(INDEX_STATE_VEL, 0);
	//先求解出旋转矩阵，再乘起来
    Eigen::Matrix3d C_nn = Sophus::SO3d::exp(X_.block<3,1>(INDEX_STATE_ORI, 0)).matrix();
    pose_.block<3,3>(0,0) = C_nn * pose_.block<3,3>(0,0);
	//加速度和角速度误差处理一下
    gyro_bias_ = gyro_bias_ - X_.block<3,1>(INDEX_STATE_GYRO_BIAS, 0);
    accel_bias_ = accel_bias_ - X_.block<3,1>(INDEX_STATE_ACC_BIAS, 0);
}

Eigen::Matrix4d ESKF::GetPose() const {
    return pose_;
}
